/// \file rigid2d.cpp
/// \brief Library for two-dimensional rigid body transformations.

#include "turtlelib/rigid2d.hpp"
#include "stdlib.h"
#include <math.h>

namespace turtlelib
{
    /// add this vector with another and store the result
  Vector2D &Vector2D::operator+=(const Vector2D &v)
  {
    x += v.x;
    y += v.y;
    // Vector2D::normalize();
    return *this;
  }

  Vector2D &Vector2D::operator-=(const Vector2D &v)
  {
    x -= v.x;
    y -= v.y;
    // Vector2D::normalize();
    return *this;
  }

  Vector2D &Vector2D::operator*=(const double scalar)
  {
    x *= scalar;
    y *= scalar;
    // Vector2D::normalize();
    return *this;
  }

  Vector2D Vector2D::normalize()
  {
    Vector2D noraml;
    if (x == 0 && y == 0)
    {
      noraml.x = 0;
      noraml.y = 0;
    }
    else
    {
      noraml.x = x / std::sqrt(std::pow(x, 2) + std::pow(y, 2));
      noraml.y = y / std::sqrt(std::pow(x, 2) + std::pow(y, 2));
    }
    return noraml;
  }

  Transform2D::Transform2D() : theta(0.0)
  {
  }

  Transform2D::Transform2D(Vector2D trans) : transform(trans), theta(0.0)
  {
  }

  Transform2D::Transform2D(double radians) : theta(radians)
  {
  }

  Transform2D::Transform2D(Vector2D trans, double radians) : transform(trans), theta(radians)
  {
  }

  Vector2D Transform2D::operator()(Vector2D v) const
  {
    Vector2D result;
    result.x = transform.x + std::cos(theta) * v.x - std::sin(theta) * v.y;
    result.y = transform.y + std::sin(theta) * v.x + std::cos(theta) * v.y;
    return result;
  }

  Transform2D Transform2D::inv() const
  {
    Transform2D result;
    result.theta = -1.0 * theta;
    result.transform.x = -1.0 * transform.x * std::cos(theta) - transform.y * std::sin(theta);
    result.transform.y = -1.0 * transform.y * std::cos(theta) + transform.x * std::sin(theta);

    return result;
  }

  Transform2D &Transform2D::operator*=(const Transform2D &rhs)
  {
    double rhs_x = rhs.transform.x;
    double rhs_y = rhs.transform.y;
    double rhs_angle = rhs.theta;

    double lhs_x = transform.x;
    double lhs_y = transform.y;
    double lhs_angle = theta;

    // Calculating the transformation vector and angle
    transform.x = lhs_x + rhs_x * cos(lhs_angle) - rhs_y * sin(lhs_angle);
    transform.y = lhs_y + rhs_x * sin(lhs_angle) + rhs_y * cos(lhs_angle);
    theta = lhs_angle + rhs_angle;
    return *this;
  }

  Vector2D Transform2D::translation() const
  {
    return transform;
  }

  double Transform2D::rotation() const
  {
    return theta;
  }

  Twist2D Transform2D::operator()(Twist2D twist) const
  {
    Twist2D twist_output;

    // Calculating the transformation swist
    twist_output.w = twist.w;
    twist_output.vx = this->transform.y * twist.w + cos(this->theta) * twist.vx -
                      sin(this->theta) * twist.vy;
    twist_output.vy = -this->transform.x * twist.w + sin(this->theta) * twist.vx +
                      cos(this->theta) * twist.vy;
    return twist_output;
  }

  Twist2D::Twist2D()
  {
    w = 0.0;
    vx = 0.0;
    vy = 0.0;
  }

  Twist2D::Twist2D(double w0, double vx0, double vy0)
  {
    w = w0;
    vx = vx0;
    vy = vy0;
  }

  std::ostream &operator<<(std::ostream &os, const Vector2D &v)
  {
    os << "[" << v.x << " " << v.y << "]";
    return os;
  }

  std::istream &operator>>(std::istream &is, Vector2D &v)
  {
    is >> v.x >> v.y;
    return is;
  }

  std::ostream &operator<<(std::ostream &os, const Transform2D &tf)
  {
    os << "deg: " << rad2deg(tf.theta) << " x: " << tf.transform.x << " y: " << tf.transform.y; // << std::endl;
    return os;
  }

  std::istream &operator>>(std::istream &is, Transform2D &tf)
  {
    Vector2D v;
    double theta;

    while (!isdigit(is.peek()))
    {
      is.get();
    }
    is >> theta;

    while (!isdigit(is.peek()))
    {
      is.get();
    }
    is >> v.x;

    while (!isdigit(is.peek()))
    {
      is.get();
    }
    is >> v.y;

    tf = Transform2D(v, deg2rad(theta));
    return is;
  }

  Transform2D operator*(Transform2D lhs, const Transform2D &rhs)
  {
    return lhs *= rhs;
  }

  std::ostream &operator<<(std::ostream &os, const Twist2D &twist)
  {
    os << "[" << twist.w << " " << twist.vx << " " << twist.vy << "]";
    return os;
  }

  std::istream &operator>>(std::istream &is, Twist2D &twist)
  {
    //std::cout << "Enter thetadot velocity (rad/s):" << std::endl;
    is >> twist.w;
    //std::cout << "Enter xdot velocity (m/s):" << std::endl;
    is >> twist.vx;
    //std::cout << "Enter ydot velocity (m/s):" << std::endl;
    is >> twist.vy;
    return is;
  }
}
